/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "usart.h"
#include "deca_device_api.h"
#include "deca_regs.h"
#include "sleep.h"
#include "deca_port.h"
#include "stdio.h"
#include "string.h"
#include "BPHero_uwb.h"
#include "queue.h"
#include "dwm1000_timestamp.h"
//#include ""
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define PRE_TIMEOUT 8
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
uint8_t cir[1016*4+1]={0};
uint8_t STATE=0;
uint8_t debugOUT[100];
uint8_t Rx_Go_Flag=0;
uint8_t DW1000_INT_FLAG=0;
uint16 register_result;
/* Time-stamps of frames transmission/reception, expressed in device time units.
 * As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
//Anchor TimeStamp
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
//Tag TimeStamp
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t final_tx_ts;
static double tof;
static double distance_Ranged = 0;

uint8_t distance_seqnum = 0;
static double distance[256] = {0};
static srd_msg_dsss *msg_f;
static srd_msg_dsss *msg_f_recv ;
static double distance_temp = 0;
uint8_t DWM_INT_FLAG=0;
int Final_Distance[MAX_ANTHOR] = {0};//save all the distance from anthor and tag
/* USER CODE END Variables */
osThreadId defaultTaskHandle;
osThreadId OLEDHandle;
osThreadId UWBHandle;
osMessageQId DW1000_INTHandle;
osSemaphoreId DW_InterruptHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void Anchor_Node_Process(void);
void Anchor_Init(void);
void Tag_Node_Process(void);
void Tag_Init(void);
void DWM1000_init(void);
void reset_DW1000(void);
/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void const * argument);
void process_display(void const * argument);
void process_UWB(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );

/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];

void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize )
{
  *ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
  *ppxIdleTaskStackBuffer = &xIdleStack[0];
  *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
  /* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* Create the semaphores(s) */
  /* definition and creation of DW_Interrupt */
  osSemaphoreDef(DW_Interrupt);
  DW_InterruptHandle = osSemaphoreCreate(osSemaphore(DW_Interrupt), 1);

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* definition and creation of DW1000_INT */
  osMessageQDef(DW1000_INT, 1, uint8_t);
  DW1000_INTHandle = osMessageCreate(osMessageQ(DW1000_INT), NULL);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* definition and creation of defaultTask */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of OLED */
  osThreadDef(OLED, process_display, osPriorityIdle, 0, 128);
  OLEDHandle = osThreadCreate(osThread(OLED), NULL);

  /* definition and creation of UWB */
  osThreadDef(UWB, process_UWB, osPriorityHigh, 0, 512);
  UWBHandle = osThreadCreate(osThread(UWB), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1000);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_process_display */
/**
* @brief Function implementing the OLED thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_process_display */
void process_display(void const * argument)
{
  /* USER CODE BEGIN process_display */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1000);
  }
  /* USER CODE END process_display */
}

/* USER CODE BEGIN Header_process_UWB */
/**
* @brief Function implementing the UWB thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_process_UWB */
void process_UWB(void const * argument)
{
  /* USER CODE BEGIN process_UWB */
	#define ANCHOR       0
	#define TAG		       0
	#define DEVICE_MODE  ANCHOR
	#define APP_NAME "SIMPLE TX v1.2"
/* Index to access to sequence number of the blink frame in the tx_msg array. */
#define BLINK_FRAME_SN_IDX 1

/* Inter-frame delay period, in milliseconds. */
#define TX_DELAY_MS 1000
//#ifdef ANCHOR_NODE
	BPhero_UWB_Message_Init();		
	DWM1000_init();	
#ifdef ANCHOR_NODE
	Anchor_Init();
#endif
	
#ifdef TAG_NODE
	Tag_Init();	
#endif
//	dwt_setrxtimeout(0);
////	dwt_enableframefilter(DWT_FF_DATA_EN);
//  dwt_rxenable(DWT_START_RX_IMMEDIATE);
//#endif
  /* Infinite loop */
  for(;;)
  {
		
		
		
#ifdef ANCHOR_NODE
//		HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_0);    
		//remain this node is Anchor
		Anchor_Node_Process();
#endif
#ifdef TAG_NODE
//		HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_1);		
		//remain this node is Tag
		Tag_Node_Process();
#endif
  }
  /* USER CODE END process_UWB */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void Anchor_Init(){
//	dwt_setpreambledetecttimeout(PRE_TIMEOUT);
}

void Anchor_Node_Process(){
	#define ANCHOR_INIT     0
	#define WAIT_FOR_POLL   1
	#define GET_POLL_TX_ACK 2
	#define WAIT_FOR_FINAL  3
	#define GET_FINAL			  4

	
	switch(STATE){
		case ANCHOR_INIT:{
			dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR));			
			HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_0);  
			dwt_setrxtimeout(0);
			dwt_enableframefilter(DWT_FF_DATA_EN);
			dwt_rxenable(DWT_START_RX_IMMEDIATE);	
			STATE=WAIT_FOR_POLL;		
			for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
			{
				rx_buffer[i] = '\0';
			}
		}		
		break;
		case WAIT_FOR_POLL:{
//			do{
//				status_reg = dwt_read32bitreg(SYS_STATUS_ID);
//			}
//			while (!((status_reg) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)));
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
      { };
			if(status_reg & SYS_STATUS_RXFCG){	
				STATE=GET_POLL_TX_ACK;
			}//end of status_reg & SYS_STATUS_RXFCG
			else{
				dwt_enableframefilter(DWT_FF_RSVD_EN);
				STATE=ANCHOR_INIT;
				dwt_rxreset();
				osDelay(500);
			}
		}
		break;
		case GET_POLL_TX_ACK:{			
			dwt_enableframefilter(DWT_FF_RSVD_EN);
			uint32 frame_len;
		 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
			if (frame_len <= RX_BUFFER_LEN)
			{
				dwt_readrxdata(rx_buffer, frame_len, 0);
				msg_f = (srd_msg_dsss*)rx_buffer;
				if(msg_f->messageData[0]!='P')
					break;				
				HAL_UART_Transmit(&huart1,"Get P\r\n",strlen("Get P\r\n"),1000);
				msg_f_send.destAddr[0] = msg_f->sourceAddr[0];
				msg_f_send.destAddr[1] = msg_f->sourceAddr[1];
				msg_f_send.seqNum = msg_f->seqNum;		
				uint32 resp_tx_time;
				poll_rx_ts = get_rx_timestamp_u64();					
//				resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
//				dwt_setdelayedtrxtime(resp_tx_time);
//				dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
//				dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS*10);
				msg_f_send.messageData[0]='A';//Poll ack message
				dwt_writetxdata(12, (uint8 *)&msg_f_send, 0);
				dwt_writetxfctrl(12, 0,1);
				
				dwt_starttx(DWT_START_TX_IMMEDIATE);//| DWT_RESPONSE_EXPECTED
				//MUST WAIT!!!!!
				while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
				 { };	
				 STATE=WAIT_FOR_FINAL;
				 dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR));
				 dwt_setrxtimeout(0);
				 dwt_enableframefilter(DWT_FF_DATA_EN);
				 dwt_rxenable(DWT_START_RX_IMMEDIATE);
				}
			else{
				STATE=ANCHOR_INIT;
				dwt_rxreset();
				osDelay(1000);
			}
		}
		break;
		case WAIT_FOR_FINAL:{
			do{
				status_reg = dwt_read32bitreg(SYS_STATUS_ID);
			}
			while (!((status_reg) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)));
			if(status_reg & SYS_STATUS_RXFCG){
				STATE=GET_FINAL;
			}
			else{
				dwt_enableframefilter(DWT_FF_RSVD_EN);
				STATE=ANCHOR_INIT;
				dwt_rxreset();
				osDelay(1000);
			}
		}
		break;
		case GET_FINAL:{
			dwt_enableframefilter(DWT_FF_RSVD_EN);
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
			if (frame_len <= RX_BUFFER_LEN)
			{
				dwt_readrxdata(rx_buffer, frame_len, 0);
				msg_f = (srd_msg_dsss*)rx_buffer;
				if(msg_f->messageData[0]!='F')
						break;
				HAL_UART_Transmit(&huart1,"Get F\r\n",strlen("Get F\r\n"),1000);			
				uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
				uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
				double Ra, Rb, Da, Db;
				int64 tof_dtu;			
				resp_tx_ts = get_tx_timestamp_u64();
				final_rx_ts = get_rx_timestamp_u64();
				final_msg_get_ts(&msg_f->messageData[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
				final_msg_get_ts(&msg_f->messageData[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
				final_msg_get_ts(&msg_f->messageData[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

				poll_rx_ts_32 = (uint32)poll_rx_ts;
				resp_tx_ts_32 = (uint32)resp_tx_ts;
				final_rx_ts_32 = (uint32)final_rx_ts;
				Ra = (double)(resp_rx_ts - poll_tx_ts);
				Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
				Da = (double)(final_tx_ts - resp_rx_ts);
				Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
				tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));

				tof = tof_dtu * DWT_TIME_UNITS;
				distance_temp = tof * SPEED_OF_LIGHT;
				sprintf(debugOUT,"Distance:%.2fm\r\n",distance_temp);
				HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
				STATE=ANCHOR_INIT;
				uint32_t I=0;
				dwt_readaccdata(cir,1016*4+1,0);
				dwt_rxreset();
//				for(I=0;I<1016;I++){
//	//					dwt_readaccdata(cir,5,4*I);
//					int16 real=0;
//					int16 imag=0;
//					real=(int16)cir[4*I+2]<<8 | (int16)cir[4*I+1];
//					imag=(int16)cir[4*I+4]<<8 | (int16)cir[4*I+3];
//					sprintf(debugOUT,"Ind,%d,R,%d,I,%d\r\n",I,real,imag);
//					HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
//					HAL_Delay(2);
//				}
				
				osDelay(5000);//delay 1s 
			}
			else{
				dwt_enableframefilter(DWT_FF_RSVD_EN);
				STATE=ANCHOR_INIT;
				dwt_rxreset();
				osDelay(2000);
			}
		}
		break;
	}
}
void Tag_Init(){	
//	dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
//  dwt_setrxtimeout(27000);//reference 51base_
//  dwt_setpreambledetecttimeout(PRE_TIMEOUT);
}
void Tag_Node_Process(){
	uint16 Dest_Address =  0x0001;
	uint32 final_tx_time;
	uint32 frame_len;
	#define TAG_INIT   			 0
	#define WAIT_FOR_ACK     1
	#define GET_ACK_TX_FINAL 2
	switch(STATE){
		case TAG_INIT:{
			dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR));
			HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_1);
			msg_f_send.destAddr[0] =(Dest_Address) &0xFF;
			msg_f_send.destAddr[1] =  ((Dest_Address)>>8) &0xFF;
			msg_f_send.seqNum = distance_seqnum;
			for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
			{
        rx_buffer[i] = '\0';
			}
			msg_f_send.messageData[0]='P';//P means Poll for the Location situation
			dwt_writetxdata(12,(uint8 *)&msg_f_send, 0) ;  // write the frame data
			dwt_writetxfctrl(12,0,1);
			dwt_starttx(DWT_START_TX_IMMEDIATE);// | DWT_RESPONSE_EXPECTED
			if(++distance_seqnum == 255)
				distance_seqnum = 0;	
			while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
			{ };
			dwt_setrxtimeout(57000);
			dwt_enableframefilter(DWT_FF_DATA_EN);
			dwt_rxenable(0);
			STATE=WAIT_FOR_ACK;
			}
			break;
		case WAIT_FOR_ACK:{
//			dwt_enableframefilter(DWT_FF_DATA_EN);
//			dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);
//			dwt_rxenable(0);
			do{
					status_reg = dwt_read32bitreg(SYS_STATUS_ID);
	//				sprintf(debugOUT,"Status:%lx\r\n",status_reg);
	//				HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
			}
			while (!((status_reg) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)));			
			if (status_reg & SYS_STATUS_RXFCG)
			{
				STATE=GET_ACK_TX_FINAL;
			}				
			else{
				STATE=TAG_INIT;
				dwt_rxreset();
				osDelay(500);
			}				
		}
		break;
		case GET_ACK_TX_FINAL:{
			dwt_enableframefilter(DWT_FF_RSVD_EN);
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
			if (frame_len <= RX_BUFFER_LEN)
			{
				dwt_readrxdata(rx_buffer, frame_len, 0);
				msg_f = (srd_msg_dsss*)rx_buffer;
				if(msg_f->messageData[0]!='A')
				break;
				HAL_UART_Transmit(&huart1,"Get A\r\n",strlen("Get A\r\n"),1000);
				
				
				msg_f_send.destAddr[0] =(Dest_Address) &0xFF;
				msg_f_send.destAddr[1] =  ((Dest_Address)>>8) &0xFF;
				msg_f_send.seqNum = msg_f->seqNum;
				
				
				poll_tx_ts = get_tx_timestamp_u64();
				resp_rx_ts = get_rx_timestamp_u64();
				final_tx_time =   dwt_readsystimestamphi32()  + 0x17cdc00/10;
				dwt_setdelayedtrxtime(final_tx_time);
				final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8);
				
				msg_f_send.messageData[0]='F';//Final message
				final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
				final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
				final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
				dwt_writetxdata(25, (uint8 *)&msg_f_send, 0) ; // write the frame data
				dwt_writetxfctrl(25,0,1);
				dwt_starttx(DWT_START_TX_DELAYED);
				while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
				{ };
				dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
				STATE=TAG_INIT;
				dwt_rxreset();				
				osDelay(1000);
			}
			else{
				dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR));
				STATE=TAG_INIT;
				dwt_rxreset();
				osDelay(1000);
			}
		}
		break;
	}
}
static dwt_config_t config = {
    2,               /* Channel number. */
    DWT_PRF_64M,     /* Pulse repetition frequency. */
    DWT_PLEN_1024,   /* Preamble length. Used in TX only. */
    DWT_PAC32,       /* Preamble acquisition chunk size. Used in RX only. */
    9,               /* TX preamble code. Used in TX only. */
    9,               /* RX preamble code. Used in RX only. */
    1,               /* 0 to use standard SFD, 1 to use non-standard SFD. */
    DWT_BR_110K,     /* Data rate. */
    DWT_PHRMODE_STD, /* PHY header mode. */
    (1025 + 64 - 32) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
	};

void reset_DW1000(void)
{
    GPIO_InitTypeDef    GPIO_InitStruct;
    // Enable GPIO used for DW1000 reset as open collector output
    GPIO_InitStruct.Pin = DW_RESET_Pin;
		GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
		GPIO_InitStruct.Pull = GPIO_PULLUP;
		GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
		HAL_GPIO_Init(DW_RESET_GPIO_Port, &GPIO_InitStruct);
		HAL_GPIO_WritePin(DW_RESET_GPIO_Port, DW_RESET_Pin, GPIO_PIN_RESET);
    deca_usleep(1);

    //put the pin back to output open-drain (not active)
    HAL_GPIO_WritePin(DW_RESET_GPIO_Port, DW_RESET_Pin, GPIO_PIN_SET);
    deca_sleep(2);
}
void DWM1000_init(void){
	HAL_NVIC_DisableIRQ(DECAIRQ_EXTI_IRQn);  
	uint32_t dwt_ID=0;
	uint8_t debugOUT[50];
  reset_DW1000();
  port_set_dw1000_slowrate();
	if(dwt_initialise(DWT_LOADUCODE) == -1)
	{
			while (1)
			{
					 HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_0|GPIO_PIN_1);
					 deca_sleep(200);
			}
	}
	dwt_ID=dwt_readdevid();
	sprintf(debugOUT,"ID:%x\r\n",dwt_ID);
	HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
	port_set_dw1000_fastrate();
	dwt_ID=dwt_readdevid();
	sprintf(debugOUT,"ID:%x\r\n",dwt_ID);
	HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
	dwt_configure(&config);
	dwt_setleds(1);
	dwt_setpanid(0xF0F0);
	dwt_setaddress16(SHORT_ADDR);
	dwt_setrxantennadelay(32950);
	dwt_settxantennadelay(0);
//	dwt_setpreambledetecttimeout(8);
//#ifdef TAG_NODE
////	dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
//	dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
//#endif
//	dwt_setinterrupt(DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
	osDelay(10);
	HAL_NVIC_EnableIRQ(DECAIRQ_EXTI_IRQn);
}
//extern uint32_t signalResetDone;

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
	if (GPIO_Pin == DW_RESET_Pin)
    {
        signalResetDone = 1;
    }
	if(GPIO_Pin == DW_IRQn_Pin){
//		DWM_INT_FLAG=1;//remind STM32 Know the DWM1000 make an interrupt
//		status_reg = dwt_read32bitreg(SYS_STATUS_ID);
//		if (status_reg & SYS_STATUS_RXFCG){
//		dwt_enableframefilter(DWT_FF_RSVD_EN);
//		status_reg=dwt_read32bitreg(SYS_STATUS_ID);
//		sprintf(debugOUT,"IT_Status:%lx\r\n",status_reg);	
//		HAL_UART_Transmit(&huart1,debugOUT,strlen(debugOUT),1000);
//		Flag=1;
////		if (status_reg & SYS_STATUS_RXFCG){
//		xQueueOverwriteFromISR(DW1000_INTHandle,&Flag,NULL);
//		}
//		else{
//			
//			dwt_enableframefilter(DWT_FF_RSVD_EN);
//		}
	}	
}
/* USER CODE END Application */
